#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <time.h>
#include <sys/time.h>
#include <pthread.h>
#include <math.h>
#include <cstring>  // 用于 memset, memcpy 等
#include <ctime>    // 用于 time_t 类型转换
#include <all/mavlink.h>
#include <common/common.h>
#include "cam_protocol.h"
#include "cam_socket.h"
#include <ros/ros.h>


#define  TEST_SEND_CAMERA_COMMAND 1


// 串口文件描述符
int serial_port;

// 相机状态
typedef enum {
    CAMERA_IDLE,
    CAMERA_CAPTURING_SINGLE,
    CAMERA_CAPTURING_TIMED,
    CAMERA_RECORDING_VIDEO
} camera_state_t;

typedef enum {
    CAMERA_CAPTURE_STATUS_VIDEO_CAPTURE_IN_IDLE = 0,
    CAMERA_CAPTURE_STATUS_VIDEO_CAPTURE_IN_PROGRESS,

} camera_video_state_t;

typedef enum {
    CAMERA_CAPTURE_STATUS_IMAGE_CAPTURE_SINGLE_IN_IDLE = 0,
    CAMERA_CAPTURE_STATUS_IMAGE_CAPTURE_SINGLE_IN_PROGRESS,
    CAMERA_CAPTURE_STATUS_IMAGE_CAPTURE_INTERVAL_IN_IDLE,
    CAMERA_CAPTURE_STATUS_IMAGE_CAPTURE_INTERVAL_IN_PROGRESS,
} camera_image_state_t;


camera_state_t camera_state = CAMERA_IDLE;
int capture_interval = 0; // 捕捉间隔（秒）
int capture_count = 0;    // 剩余捕捉次数
int image_count = 0;      // 图像计数器
int video_recording_time = 0; // 视频录制时间（秒）
int recording_start_time = 0; // 录制开始时间


CAMERA_MODE current_mode = CAMERA_MODE_IMAGE;

#define  DEFAULT_CAMERA_SYSTEM_ID 1

//compid
#define DEFAULT_COMMENT_ID MAV_COMP_ID_CAMERA
//
#define PTZ_CAMERA_DEVICE_ID 1

uint8_t recv_buf[MAVLINK_MAX_PACKET_LEN] = {0};
ssize_t recv_len = 0;

void send_camera_information();
void send_camera_setting();



// 初始化串口
int init_serial(const char *port) {
    int fd = open(port, O_RDWR | O_NOCTTY | O_NONBLOCK);  // 设置为非阻塞
    if (fd == -1) {
        ROS_ERROR("Cannot open UART");
        return -1;
    }

    struct termios options;
    tcgetattr(fd, &options);

    cfsetispeed(&options, B115200);
    cfsetospeed(&options, B115200);

    options.c_cflag &= ~PARENB;
    options.c_cflag &= ~CSTOPB;
    options.c_cflag &= ~CSIZE;
    options.c_cflag |= CS8;

    options.c_cflag |= (CLOCAL | CREAD);
    options.c_cflag &= ~CRTSCTS;

    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
    options.c_oflag &= ~OPOST;

    options.c_cc[VMIN] = 0;
    options.c_cc[VTIME] = 0;

    tcsetattr(fd, TCSANOW, &options);
    return fd;
}

// 打印十六进制数据
void print_hex(const char *prefix, const uint8_t *data, size_t len) {
    ROS_INFO("%s: ", prefix);
    for (size_t i = 0; i < len; i++) {
        ROS_INFO("%02X ", data[i]);
        if ((i+1) % 16 == 0 && (i+1) < len) ROS_INFO("\n        ");
    }
    ROS_INFO("\n");
}

// 发送MAVLink消息
void send_mavlink_message(mavlink_message_t *msg) {
    uint8_t buf[MAVLINK_MAX_PACKET_LEN];
    uint16_t len = mavlink_msg_to_send_buffer(buf, msg);

    // 打印发送的原始数据
    // print_hex("======================发送原始数据", buf, len);

    if (write(serial_port, buf, len) != len)
    {
        ROS_ERROR("send message failed\n");

    }
}

// 发送命令确认消息
void send_command_ack(uint16_t command, uint8_t result) {
    mavlink_message_t msg;
    mavlink_command_ack_t ack = {};
    ack.command = command;
    ack.result = result;

    mavlink_msg_command_ack_encode(DEFAULT_CAMERA_SYSTEM_ID, DEFAULT_COMMENT_ID, &msg, &ack);
    send_mavlink_message(&msg);
    ROS_INFO("send command ACK: command=%d, result=%d\n", command, result);
}

// 发送图像捕捉确认消息
void send_image_captured(uint32_t time_boot_ms, uint32_t image_index) {
    mavlink_message_t msg;
    mavlink_camera_image_captured_t captured = {};
    captured.time_boot_ms = time_boot_ms;
    captured.time_utc = time(NULL);
    captured.camera_id = 0;
    captured.lat = 0;
    captured.lon = 0;
    captured.alt = 0;
    captured.relative_alt = 0;
    memset(captured.q, 0, sizeof(captured.q));
    captured.image_index = image_index;
    captured.capture_result = 0;
    strncpy(captured.file_url, "", sizeof(captured.file_url));

    mavlink_msg_camera_image_captured_encode(DEFAULT_CAMERA_SYSTEM_ID, DEFAULT_COMMENT_ID, &msg, &captured);
    send_mavlink_message(&msg);
    ROS_INFO("send image capture ACK: index=%u\n", image_index);
}

// 发送相机拍摄状态
void send_camera_capture_status(uint32_t time_boot_ms) {
    mavlink_message_t msg;
    mavlink_camera_capture_status_t status = {0};

    // 设置状态标志
    if (camera_state == CAMERA_RECORDING_VIDEO) {
        status.video_status = CAMERA_CAPTURE_STATUS_VIDEO_CAPTURE_IN_IDLE;
    }
    // else if (camera_state != CAMERA_IDLE) {
    //     status.image_status = CAMERA_CAPTURE_STATUS_IMAGE_CAPTURE_SINGLE_IN_PROGRESS;
    // }
    else if (camera_state == CAMERA_CAPTURING_SINGLE)
    {
        status.image_status = CAMERA_CAPTURE_STATUS_IMAGE_CAPTURE_SINGLE_IN_IDLE;
    }
    else if (camera_state == CAMERA_CAPTURING_TIMED)
    {
        status.image_status = CAMERA_CAPTURE_STATUS_IMAGE_CAPTURE_INTERVAL_IN_IDLE;
    }


    // 设置当前状态
    status.time_boot_ms = time_boot_ms;
    status.image_interval = capture_interval;
    status.recording_time_ms = video_recording_time * 1000;
    status.image_count = image_count;

    mavlink_msg_camera_capture_status_encode(DEFAULT_CAMERA_SYSTEM_ID, DEFAULT_COMMENT_ID, &msg, &status);
    send_mavlink_message(&msg);
    ROS_INFO("send camera capture status: ");

    if (camera_state == CAMERA_RECORDING_VIDEO) {
        ROS_INFO("Recording (%d sec)\n", video_recording_time);
    } else if (camera_state != CAMERA_IDLE) {
        ROS_INFO("Taking photo (%d remaining)\n", capture_count);
    } else {
        ROS_INFO("IDLE\n");
    }
}

// 解析并处理MAVLink消息
void parse_and_handle_message(mavlink_message_t *msg) {
    if( msg->msgid == 0 || msg->msgid == 233)
    {
        return;
    }
    ROS_INFO("=====================recv MAVLink message:===============================\n");
    ROS_INFO("  MSG ID: %d\n", msg->msgid);
    ROS_INFO("  SYS ID: %d, compid: %d\n", msg->sysid, msg->compid);
    ROS_INFO("  SEQ: %d\n", msg->seq);
    ROS_INFO("  LEN: %d\n", msg->len);


    // 根据消息ID进行处理
    switch (msg->msgid) {
    case MAVLINK_MSG_ID_HEARTBEAT: {
        mavlink_heartbeat_t hb;
        mavlink_msg_heartbeat_decode(msg, &hb);
        ROS_INFO("  Heartbeat decoded:\n");
        ROS_INFO("    Type: %d\n", hb.type);
        ROS_INFO("    Autopilot: %d\n", hb.autopilot);
        ROS_INFO("    Base mode: 0x%X\n", hb.base_mode);
        ROS_INFO("    System status: %d\n", hb.system_status);
        ROS_INFO("    MAVLink version: %d\n", hb.mavlink_version);
        break;
    }

    case MAVLINK_MSG_ID_COMMAND_LONG:
    {
        mavlink_command_long_t cmd;
        mavlink_msg_command_long_decode(msg, &cmd);
        ROS_INFO("  Command decoded:\n");
        ROS_INFO("    Command ID: %d\n", cmd.command);
        ROS_INFO("    Target system ID: %d\n", cmd.target_system);
        ROS_INFO("    Target component ID: %d\n", cmd.target_component);
        ROS_INFO("    Confirmation: %d\n", cmd.confirmation);
        ROS_INFO("    Params: [%.1f, %.1f, %.1f, %.1f, %.1f, %.1f, %.1f]\n",
                 cmd.param1, cmd.param2, cmd.param3,
                 cmd.param4, cmd.param5, cmd.param6, cmd.param7);
        if(cmd.target_component != DEFAULT_COMMENT_ID)
        {
            ROS_INFO("Component ID is not gimbal camera, ignoring\n");
            break;
        }
        // 打印接收的原始数据
        // print_hex("recv raw data", recv_buf, recv_len);

        // 处理图像捕捉命令
        switch(cmd.command)
        {
        case MAV_CMD_IMAGE_START_CAPTURE:
        {
            // 参数1: 间隔时间（秒），0表示只拍一张
            // 参数2: 拍摄张数，0表示无限
            uint8_t targetCameraID = (uint8_t)cmd.param1;
            capture_interval = (int)cmd.param2;
            capture_count = (cmd.param3 > 0) ? (int)cmd.param3 : 0; // -1表示无限
            int sequence_num = cmd.param4;
            if (capture_interval == 0) {
                camera_state = CAMERA_CAPTURING_SINGLE;
            } else {
                camera_state = CAMERA_CAPTURING_TIMED;
            }

            // 发送命令确认
            send_command_ack(cmd.command, MAV_RESULT_ACCEPTED);
            ROS_INFO("Start image capture: Interval=%ds, Count=%d\n", capture_interval, capture_count);
            break;
        }
        case MAV_CMD_IMAGE_STOP_CAPTURE:
        {
            camera_state = CAMERA_IDLE;
            send_command_ack(cmd.command, MAV_RESULT_ACCEPTED);
            ROS_INFO("Stop image capture\n");
            break;
        }
        case MAV_CMD_VIDEO_START_CAPTURE:
        {
            // 参数2: 录制时间（秒），0表示无限
            video_recording_time = 0;
            camera_state = CAMERA_RECORDING_VIDEO;
            recording_start_time = time(NULL);
            send_command_ack(cmd.command, MAV_RESULT_ACCEPTED);
            ROS_INFO("Start video recording\n");
            break;
        }
        case MAV_CMD_VIDEO_STOP_CAPTURE:
        {
            camera_state = CAMERA_IDLE;
            send_command_ack(cmd.command, MAV_RESULT_ACCEPTED);
            ROS_INFO("Stop video recording\n");
            break;
        }
        case MAV_CMD_SET_CAMERA_MODE:
        {
            CAMERA_MODE new_mode = (CAMERA_MODE)cmd.param2;

            if (new_mode >= CAMERA_MODE_IMAGE && new_mode <= CAMERA_MODE_IMAGE_SURVEY) {
                current_mode = new_mode;
                send_command_ack(cmd.command, MAV_RESULT_ACCEPTED);
                ROS_INFO("Camera mode set to: ");
                switch (current_mode) {
                case CAMERA_MODE_IMAGE:
                    ROS_INFO("Image mode\n");
                    break;
                case CAMERA_MODE_VIDEO:
                    ROS_INFO("Video mode\n");
                    break;
                case CAMERA_MODE_IMAGE_SURVEY:
                    ROS_INFO("Survey mode\n");
                    break;
                }
            }
            else {
                send_command_ack(cmd.command, MAV_RESULT_UNSUPPORTED);
                ROS_INFO("Unsupported camera mode: %d\n", new_mode);
            }
            break;
        }
        case MAV_CMD_REQUEST_MESSAGE:
        {
            uint32_t message_id = (uint32_t)cmd.param1;

            if (message_id == MAVLINK_MSG_ID_CAMERA_INFORMATION)
            {
                send_command_ack(cmd.command, MAV_RESULT_ACCEPTED);
                send_camera_information();
                ROS_INFO("Sent camera information (response to request)\n");
            }
            else if(message_id == MAVLINK_MSG_ID_CAMERA_SETTINGS)
            {
                // 仅处理目标为摄像头的设置消息
                send_command_ack(cmd.command, MAV_RESULT_ACCEPTED);
                send_camera_setting();
                ROS_INFO("Sent camera settings (response to request)\n");

            }
            else if(message_id == MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS)
            {
                // 仅处理目标为摄像头的设置消息

                send_command_ack(cmd.command, MAV_RESULT_ACCEPTED);
                send_camera_capture_status(0);
                ROS_INFO("Sent camera capture status (response to request)\n");

            }
            else
            {
                send_command_ack(cmd.command, MAV_RESULT_UNSUPPORTED);
                ROS_WARN("Unsupported requested message ID: %d\n", message_id);
            }
            break;
        }
        default:
            ROS_ERROR("Unsupported command ID: %d\n", cmd.command);

        }
        break;
    }
    case MAVLINK_MSG_ID_CAMERA_INFORMATION: {
        ROS_INFO("  Received camera information request\n");
        // 实际应用中应发送相机信息
        send_camera_information();
        break;
    }

    case MAVLINK_MSG_ID_CAMERA_SETTINGS: {
        mavlink_camera_settings_t settings;
        mavlink_msg_camera_settings_decode(msg, &settings);
        ROS_INFO("  Camera settings decoded:\n");
        ROS_INFO("    Mode: %d\n", settings.mode_id);
        ROS_INFO("    Zoom level: %.1f\n", settings.zoomLevel);
        ROS_INFO("    Focus level: %.1f\n", settings.focusLevel);
        break;
    }

    default:
        ROS_WARN("  Unknown message type\n");
        break;
    }
}

// 创建并发送心跳包
void send_heartbeat() {
    mavlink_message_t msg;
    mavlink_heartbeat_t hb = {};
    hb.type = MAV_TYPE_CAMERA;
    hb.autopilot = MAV_AUTOPILOT_INVALID;
    hb.base_mode = 0;
    hb.custom_mode = 0;
    hb.system_status = (camera_state == CAMERA_IDLE) ? MAV_STATE_STANDBY : MAV_STATE_ACTIVE;
    hb.mavlink_version = 3;

    mavlink_msg_heartbeat_encode(DEFAULT_CAMERA_SYSTEM_ID, DEFAULT_COMMENT_ID, &msg, &hb);
    send_mavlink_message(&msg);
    // ROS_INFO("发送心跳包\n");
}

// 创建并发送CAMERA_INFORMATION消息
void send_camera_information()
{
    mavlink_message_t msg;
    mavlink_camera_information_t cam_info = {0};

    // 填充消息内容
    cam_info.time_boot_ms = 0;

    // 固件版本: Major=1, Minor=2, Patch=3, Dev=4
    cam_info.firmware_version = (4 << 24) | (3 << 16) | (2 << 8) | 1;

    cam_info.focal_length = 3.5f; // 3.5mm焦距
    cam_info.sensor_size_h = 6.17f; // 传感器水平尺寸(mm)
    cam_info.sensor_size_v = 4.55f; // 传感器垂直尺寸(mm)

    // 标志: 支持的功能
    cam_info.flags = CAMERA_CAP_FLAGS_CAPTURE_IMAGE |
                     CAMERA_CAP_FLAGS_CAPTURE_VIDEO |
                     CAMERA_CAP_FLAGS_HAS_MODES;

    cam_info.resolution_h = 1920;
    cam_info.resolution_v = 1080;
    cam_info.cam_definition_version = 1;

    // 供应商名称
    const char *vendor_name = "369 Camera Vendor";
    memcpy(cam_info.vendor_name, vendor_name, strlen(vendor_name));

    // 型号名称
    const char *model_name = "369 Pro Camera";
    memcpy(cam_info.model_name, model_name, strlen(model_name));

    cam_info.lens_id = 1;

    // 定义URL
    const char *definition_uri = NULL;
    if(definition_uri)
    {
        strncpy(cam_info.cam_definition_uri, definition_uri, sizeof(cam_info.cam_definition_uri));

    }
    else
    {   ///////////////////////////////////////
        memset(cam_info.cam_definition_uri,0,sizeof(cam_info.cam_definition_uri));
    }

    cam_info.gimbal_device_id = 0;
    /*Camera id of a non-MAVLink camera attached to an autopilot (1-6).
    0 if the component is a MAVLink camera (with its own component id).
    */
    cam_info.camera_device_id = 0;

    // 打包并发送消息
    mavlink_msg_camera_information_encode(DEFAULT_CAMERA_SYSTEM_ID, DEFAULT_COMMENT_ID, &msg, &cam_info);
    send_mavlink_message(&msg);
    ROS_DEBUG("Send CAMERA_INFORMATION msg\n");
}

void send_camera_setting()
{
    mavlink_message_t msg;

    mavlink_camera_settings_t camera_settings = {0};
    camera_settings.camera_device_id = 0;
    camera_settings.focusLevel = 0;
    camera_settings.zoomLevel = 0;
    camera_settings.time_boot_ms = 0;
    camera_settings.mode_id = current_mode;
    // 打包并发送消息
    mavlink_msg_camera_settings_encode(DEFAULT_CAMERA_SYSTEM_ID, DEFAULT_COMMENT_ID, &msg, &camera_settings);
    send_mavlink_message(&msg);
    ROS_INFO("Send CAMERA_SETTINGS msg\n");
}

// 处理图像捕捉逻辑
void handle_image_capture(struct timeval *now) {
    static struct timeval last_capture = {0, 0};
    uint32_t time_boot_ms = now->tv_sec * 1000 + now->tv_usec / 1000;

    // 更新录制时间（如果是视频模式）
    if (camera_state == CAMERA_RECORDING_VIDEO) {
        video_recording_time = now->tv_sec - recording_start_time;
    }

    if (camera_state == CAMERA_IDLE) {
        return;
    }

    // 计算自上次捕捉以来的时间
    double elapsed = (now->tv_sec - last_capture.tv_sec) +
                     (now->tv_usec - last_capture.tv_usec) / 1000000.0;

    // 检查是否应该捕捉图像
    if ((camera_state == CAMERA_CAPTURING_SINGLE) ||
            (camera_state == CAMERA_CAPTURING_TIMED && elapsed >= capture_interval)) {

        // 模拟图像捕捉
#if TEST_SEND_CAMERA_COMMAND
        SendCommandTakePhoto(nullptr);

#endif
        ROS_INFO("=========Capture Image #%d====\n", image_count + 1);

        // 发送图像捕捉确认
        send_image_captured(time_boot_ms, image_count);

        // 更新状态
        last_capture = *now;
        image_count++;

        // 更新剩余捕捉次数
        if (capture_count > 0) {
            capture_count--;
            if (capture_count == 0) {
                camera_state = CAMERA_IDLE;
                ROS_INFO("Complete the specified number of image captures\n");
            }
        }
    }
}

// 主循环
int main(int argc, char **argv)
{
    ros::init(argc, argv, "ros_cam_manager");
    cam_socket_init();
    SendCommandSetCameraMode(nullptr);

    serial_port = init_serial("/dev/ttyS4");
    if (serial_port < 0) {
        return 1;
    }

    ROS_INFO("============Camera manager initialized======================");
    ROS_INFO("SYS ID: 1, Comment ID: %d", DEFAULT_COMMENT_ID);

    mavlink_status_t status;

    struct timeval last_send, now;
    gettimeofday(&last_send, NULL);

    int capture_status_counter = 0;

    while (1) {
        gettimeofday(&now, NULL);
        uint32_t time_boot_ms = now.tv_sec * 1000 + now.tv_usec / 1000;

        // 使用 select 检查串口是否有数据
        fd_set read_fds;
        FD_ZERO(&read_fds);
        FD_SET(serial_port, &read_fds);

        struct timeval timeout;
        timeout.tv_sec = 0;
        timeout.tv_usec = 100000; // 100ms

        int ret = select(serial_port + 1, &read_fds, NULL, NULL, &timeout);
        if (ret > 0 && FD_ISSET(serial_port, &read_fds)) {
            recv_len = read(serial_port, recv_buf, sizeof(recv_buf));
            if (recv_len > 0) {
                for (ssize_t i = 0; i < recv_len; i++) {
                    mavlink_message_t msg;
                    if (mavlink_parse_char(MAVLINK_COMM_0, recv_buf[i], &msg, &status)) {
                        parse_and_handle_message(&msg);
                    }
                }
            }
        }

        handle_image_capture(&now);

        double elapsed = (now.tv_sec - last_send.tv_sec) +
                         (now.tv_usec - last_send.tv_usec) / 1000000.0;

        if (elapsed >= 1.0) {
            send_heartbeat();
            capture_status_counter++;
            if (capture_status_counter % 5 == 0) {
                send_camera_information();
            }
            last_send = now;
        }

    }

    close(serial_port);
    return 0;
}